/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalSource;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SimpleRobot;

import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;

import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import java.io.PrintStream;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the
 * resource directory.
 */
public class RobotTTest1 extends SimpleRobot {

    private static final boolean IS_JAGUAR_MOTORS = true;

    private static final PrintStream DEBUGGER_OUT = System.out;

    private static final boolean DEBUGGER_ON = true;
    private static final boolean CAMERA_SYS_ON = true;
    private static final boolean TRACKING_SYS_ON = false;
    private static final boolean PNEUMATICS_SYS_ON = true;
    private static final boolean BALL_CONTROL_SYS_ON = true;
    private static final boolean ENCODER_SYS_ON = false;
    private static final boolean HANGER_SYS_ON = false;

    private static final double DELAY = 0.001; // Loop delay (s)

    private static final byte MAIN_SLOT = 4;	// main slot for sidecar

    // Victor motor setup for Pre-Season Bot
    private static final byte PWM_VIC_LA = 1, // VICTOR FRONT LEFT
                              PWM_VIC_LB = 3, // VICTOR REAR LEFT
                              PWM_VIC_RA = 2, // VICTOR FRONT RIGHT
                              PWM_VIC_RB = 4;  // VICTOR REAR  RIGHT

    // Jaguar motor setup for Season Bot
    private static final byte PWM_JAG_RA = 1, // JAGUAR A RIGHT
//                    				    PWM_JAG_RB = 4,	// JAGUAR B RIGHT
                              PWM_JAG_LA = 2;	// JAGUAR A LEFT
//                            PWM_JAG_LB = 2;	// JAGUAR B LEFT

    // Joystick on USB
    private static final byte USB_JOYSTICK = 1;
    private static final byte USB_HANGSTICK = 2;

    // Encoders for actual wheel motion detection
    private static final byte DIO_ENC_RA = 4, // Digital IO for Encoder Right A
                              DIO_ENC_RB = 5, // DIO for Encoder Right B
                              DIO_ENC_LA = 2, // DIO for Encoder Left A
                              DIO_ENC_LB = 3; // DIO for Encoder Left B

    // Joystick buttons
    private static final byte TRIGGER = 1, // Fire   firing mechanism
                              KICK_ALT = 11,
                              RETRACT_ALT = 10,
                              BALL_CONTROL_OFF = 4, // off ball control mech
                              BALL_CONTROL_ON = 5, // on ball control mech
                              FULL_FORWARD_ON = 6,
                              FULL_FORWARD_OFF = 7,
                              ALIGN = 8;	  // aligning button
    
    private static final byte MODE_HANG_ON = 8,
                              MODE_HANG_OFF = 9;

    
    // Compessor for firing mechanism
    private static final byte DIO_PRESSURE_SWITCH = 1, // Pressure switch DIO
                              RELAY_COMPRESSOR = 1;// Comp. Relay channel

    // Relay for kicker channel
    private static final byte RELAY_KICKER_FIRE = 4,
                              RELAY_KICKER_RETRACT = 3,
                              RELAY_BALL_CONTROL = 2;

    // Gyro channel
    private static final byte GYRO = 1;
    private static final byte GYRO_SLOT = 1;

    private static final double JOY_Y_MAX = 1.0; // joystick max magnitude y axis


    // Ball controller "on" direction
    private static final Relay.Direction BALL_CON_DIRECTION =
                                                       Relay.Direction.kForward;

    
    // For Alignment Copied Code
    private static final double kScoreThreshold = .01;

    // winch for hanging
    private static final byte   PWM_HANGER = 3;

    /** Debugger **/
    private static final byte   DEBUG_DRIVE = 1,
                                DEBUG_JOYSTICK = 2,
                                DEBUG_COMPRESSOR = 3,
                                DEBUG_PNEUMATICS = 4,
                                DEBUG_BALL_CONTROL = 5,
                                DEBUG_CAMERA = 6,
                                DEBUG_GYRO = 7,
                                DEBUG_TRACKING = 8,
                                DEBUG_TRACKING_PIDCONTROLLER = 9,
                                DEBUG_ENCODERS = 10,
                                DEBUG_HANGER = 11;
    
    private static final boolean D_L_01 = false; // Drive
    private static final boolean D_L_02 = false; // Joystick
    private static final boolean D_L_03 = false; // Compressor
    private static final boolean D_L_04 = false; // Pneumatics
    private static final boolean D_L_05 = false; // Ball Controller
    private static final boolean D_L_06 = true;  // Camera
    private static final boolean D_L_07 = false; // Gyro
    private static final boolean D_L_08 = false; // Tracking
    private static final boolean D_L_09 = false; // Tracking PIDController
    private static final boolean D_L_10 = false; // Encoders
    private static final boolean D_L_11 = true;  // Hanger
    private static final boolean D_L_12 = false;
    private static final boolean D_L_13 = false;
    private static final boolean D_L_14 = false;
    private static final boolean D_L_15 = false;
    private static final boolean D_L_16 = false;

    private static boolean[] DEBUG_LEVELS = {D_L_01,
                                             D_L_02,
                                             D_L_03,
                                             D_L_04,
                                             D_L_05,
                                             D_L_06,
                                             D_L_07,
                                             D_L_08,
                                             D_L_09,
                                             D_L_10,
                                             D_L_11,
                                             D_L_12,
                                             D_L_13,
                                             D_L_14,
                                             D_L_15,
                                             D_L_16};


    private static final String D_S_01 = "Drive";
    private static final String D_S_02 = "Joystick";
    private static final String D_S_03 = "Compressor";
    private static final String D_S_04 = "Pneumatics";
    private static final String D_S_05 = "Ball Controller";
    private static final String D_S_06 = "Camera";
    private static final String D_S_07 = "Gyro";
    private static final String D_S_08 = "Tracking";
    private static final String D_S_09 = "Tracking PIDController";
    private static final String D_S_10 = "Encoders";
    private static final String D_S_11 = "Hanger";
    private static final String D_S_12 = "None";
    private static final String D_S_13 = "None";
    private static final String D_S_14 = "None";
    private static final String D_S_15 = "None";
    private static final String D_S_16 = "None";

    private static String[] DEBUG_STRINGS = {D_S_01,
                                             D_S_02,
                                             D_S_03,
                                             D_S_04,
                                             D_S_05,
                                             D_S_06,
                                             D_S_07,
                                             D_S_08,
                                             D_S_09,
                                             D_S_10,
                                             D_S_11,
                                             D_S_12,
                                             D_S_13,
                                             D_S_14,
                                             D_S_15,
                                             D_S_16};
    /** End debugger **/

    /***************************************************************************
     ****************************** END CONSTANTS ******************************
     **************************************************************************/
    Debugger d;

    RobotDrive drive;
    Joystick joy;


    // Encoders for finding wheel rotations
    DigitalSource encChRightA, encChRightB;
    DigitalSource encChLeftA, encChLeftB;
    Encoder encRight;
    Encoder encLeft;

    // Air compressor for solenoids
    Compressor compressor;

    // Kicker solenoids
    Relay kickSolFire, kickSolRetract;

    // Camera
    AxisCamera cam;

    // Ball control mechanism
    Relay ballController;

    // Gyro for alignment
    AnalogChannel gyroChannel;
    Gyro gyro;
    TrackerDashboard trackerDashboard;
    PIDController turnController;
    boolean lastTrigger;

    // Full forward crap
    boolean fullForwardEnabled;

    // elevator and winch
    SpeedController hanger;
    boolean isHangMode   = false;

    RobotDrive hangerDrive;
    Joystick hangStick;

    public RobotTTest1() {
        System.out.println("STARTING");
        getWatchdog().setExpiration(0.1);

        d = new Debugger(DEBUGGER_OUT, DEBUGGER_ON, DEBUG_LEVELS,
                                                            DEBUG_STRINGS);

        if (IS_JAGUAR_MOTORS) { // using jaguars
            d.dbg(DEBUG_DRIVE, "Instantiating drive with 2 Jaguars");
            d.dbg(DEBUG_DRIVE, "cRIO Slot: " + MAIN_SLOT);
            d.dbg(DEBUG_DRIVE, "Left PWM: " + PWM_JAG_LA);
            d.dbg(DEBUG_DRIVE, "Right PWM: " + PWM_JAG_RA);
            
            drive = new RobotDrivePlus(
                new Jaguar(MAIN_SLOT, PWM_JAG_LA),
                new Jaguar(MAIN_SLOT, PWM_JAG_RA)
//              new Jaguar(MAIN_SLOT, PWM_JAG_RA),
//              new Jaguar(MAIN_SLOT, PWM_JAG_RB)
                );
        } else { // using victors
            d.dbg(DEBUG_DRIVE, "Instantiating drive with 4 Victors");
            d.dbg(DEBUG_DRIVE, "cRIO Slot: " + MAIN_SLOT);
            d.dbg(DEBUG_DRIVE, "Left A PWM: " + PWM_VIC_LA);
            d.dbg(DEBUG_DRIVE, "Left B PWM: " + PWM_VIC_LB);
            d.dbg(DEBUG_DRIVE, "Right A PWM: " + PWM_VIC_RA);
            d.dbg(DEBUG_DRIVE, "Right B PWM: " + PWM_VIC_RB);
            
            drive = new RobotDrivePlus(
                new Victor(MAIN_SLOT, PWM_VIC_LA),
                new Victor(MAIN_SLOT, PWM_VIC_LB),
                new Victor(MAIN_SLOT, PWM_VIC_RA),
                new Victor(MAIN_SLOT, PWM_VIC_RB)
                );
        }

        d.dbg(DEBUG_JOYSTICK, "Instantiating joystick on USB " + USB_JOYSTICK);
        joy = new Joystick(USB_JOYSTICK);

        if(ENCODER_SYS_ON) {
            initEncoderSystem();
        }

        if(PNEUMATICS_SYS_ON) {
            initPneumaticsSystem();
        }

            System.out.println("Debug 0");
        if(CAMERA_SYS_ON) {
            System.out.println("Debug 1");
            initCameraSystem();
        }


        if(BALL_CONTROL_SYS_ON) {
            initBallControlSystem();
        }

        if(TRACKING_SYS_ON) {
            initTrackingSystem();
        }

        if(HANGER_SYS_ON) {
            initHangerSystem();
        }
    }

    private void initHangerSystem() {
        // elevator
        hanger    = new Victor(MAIN_SLOT, PWM_HANGER);

        hangStick   = new Joystick(USB_HANGSTICK);
        hangerDrive = new RobotDrive(hanger, null);
    }

    private void initPneumaticsSystem() {
        d.dbg(DEBUG_PNEUMATICS, "Instantiating pneumatics system");

        //compressor
        d.dbg(DEBUG_PNEUMATICS, "Instantiating compressor");
        d.dbg(DEBUG_PNEUMATICS, "cRIO Slot (Pressure Switch): " + MAIN_SLOT);
        d.dbg(DEBUG_PNEUMATICS, "Pressure Switch DIO: " + DIO_PRESSURE_SWITCH);
        d.dbg(DEBUG_PNEUMATICS, "cRIO Slot (Compressor): " + MAIN_SLOT);
        d.dbg(DEBUG_PNEUMATICS, "Compressor Relay: " + RELAY_COMPRESSOR);
        compressor = new Compressor(MAIN_SLOT, DIO_PRESSURE_SWITCH, MAIN_SLOT,
                                                            RELAY_COMPRESSOR);

        // Kicker Relays
        d.dbg(DEBUG_PNEUMATICS, "Instantiating kicking solenoids");
        d.dbg(DEBUG_PNEUMATICS, "cRIO Slot (Fire): " + MAIN_SLOT);
        d.dbg(DEBUG_PNEUMATICS, "Kicker Fire Relay: " + RELAY_KICKER_FIRE);
        kickSolFire = new Relay(MAIN_SLOT, RELAY_KICKER_FIRE);

        d.dbg(DEBUG_PNEUMATICS, "cRIO Slot (Retract): " + MAIN_SLOT);
        d.dbg(DEBUG_PNEUMATICS, "Kicker Retract Relay: " +RELAY_KICKER_RETRACT);
        kickSolRetract = new Relay(MAIN_SLOT, RELAY_KICKER_RETRACT);
    }

    private void initBallControlSystem() {
        d.dbg(DEBUG_BALL_CONTROL, "Instantiating ball control system");
        d.dbg(DEBUG_BALL_CONTROL, "cRIO Slot: " + MAIN_SLOT);
        d.dbg(DEBUG_BALL_CONTROL, "Ball Control Relay: " + RELAY_BALL_CONTROL);
        ballController = new Relay(MAIN_SLOT, RELAY_BALL_CONTROL,
                                                            BALL_CON_DIRECTION);
        
        d.dbg(DEBUG_BALL_CONTROL, "Initial status: off");
        ballController.set(Relay.Value.kOff);
    }

    private void initEncoderSystem() {
        d.dbg(DEBUG_ENCODERS, "Instantiating encoder system");
        d.dbg(DEBUG_ENCODERS, "cRIO Slot: " + MAIN_SLOT);

        // encoders
        d.dbg(DEBUG_ENCODERS, "Instantiating right encoder");
        d.dbg(DEBUG_ENCODERS, "Right encoder DIO channel A: " + DIO_ENC_RA);
        d.dbg(DEBUG_ENCODERS, "Right encoder DIO channel B: " + DIO_ENC_RB);
        
        encRight = new Encoder(new DigitalInput(MAIN_SLOT, DIO_ENC_RA),
                                    new DigitalInput(MAIN_SLOT, DIO_ENC_RB));

        d.dbg(DEBUG_ENCODERS, "Instantiating left encoder");
        d.dbg(DEBUG_ENCODERS, "Left encoder DIO channel A: " + DIO_ENC_LA);
        d.dbg(DEBUG_ENCODERS, "Left encoder DIO channel B: " + DIO_ENC_LB);

        encLeft = new Encoder(new DigitalInput(MAIN_SLOT, DIO_ENC_LA),
                                    new DigitalInput(MAIN_SLOT, DIO_ENC_LB));
    }

    private void initTrackingSystem() {
        d.dbg(DEBUG_TRACKING, "Instantiating tracking system");

        lastTrigger = false;
        trackerDashboard = new TrackerDashboard();
        
        d.dbg(DEBUG_GYRO, "Instantiating gyro");
        d.dbg(DEBUG_GYRO, "cRIO Slot: " + GYRO_SLOT);
        d.dbg(DEBUG_GYRO, "Analog Breakout: " + GYRO);
        
        gyroChannel = new AnalogChannel(GYRO_SLOT, GYRO);
        gyro = new Gyro(gyroChannel);

        double cP = 0.10, cI = 0.08, cD = 0.18;
        d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Instantiating PIDController " +
                                                            "turn controller");
        d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Constants are as follows");
        d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "cP: " + cP);
        d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "cI: " + cI);
        d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "cD: " + cD);
        
        turnController = new PIDController(cP, cI, cD, gyro, new PIDOutput() {

            public void pidWrite(double output) {
                drive.arcadeDrive(0, output);
                d.dbg(DEBUG_TRACKING_PIDCONTROLLER,
                                        "Attempting to turn");
                d.dbg(DEBUG_TRACKING_PIDCONTROLLER,
                                        "Turn angle: " + output);
                drive.arcadeDrive(0.0, output);
            }
        }, .005);
        gyro.setSensitivity(.007);
        gyro.reset();
        
        turnController.setInputRange(-360.0, 360.0);
        turnController.setTolerance(1 / 90. * 100);
        turnController.disable();
    }

    private void initCameraSystem() {
        d.dbg(DEBUG_CAMERA, "Instantiating camera");
        d.dbg(DEBUG_CAMERA, "Resolution 320x240");
        
        cam = AxisCamera.getInstance();
        cam.writeResolution(AxisCamera.ResolutionT.k320x240);
        cam.writeBrightness(0);

        cam.writeCompression(75); // JPEG Compression: 0-100; 0 least compressed
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        getWatchdog().setEnabled(false);
        /**
         *	NO AUTONOMOUS CODE YET.
         *	TODO: IMPLEMENT AUTONOMOUS MODE
         */
    }


    double cErrR = 0.0, cErrL = 0.0; // for full forward system
    Thread fireThread;

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        getWatchdog().setEnabled(true);

        startPneumatics();

        startEncoders();

        while (true && isOperatorControl() && isEnabled()) {//&& true?wtf...wtf
            getWatchdog().feed();

            doJoystickDataOps();
            doEncoderDataOps();
            doTrackingDataOps();

            doAlternateFiringOps();
            doAlternateBallControlOps();

            doRegularFiringOps();

            doTrackingAlignOps();

            doFullForwardOps();

            doCameraOps();

            doHangerOps();

            Timer.delay(DELAY);
        }

    }

    public void startPneumatics() {
        if(PNEUMATICS_SYS_ON) {
            d.dbg(DEBUG_COMPRESSOR, "Starting compressor");
            compressor.start();

            d.dbg(DEBUG_PNEUMATICS, "Instantiating initial firing thread");
            fireThread = new FireThread(kickSolFire, kickSolRetract, ballController);
        }
    }

    public void startEncoders() {
        if(ENCODER_SYS_ON) {
            d.dbg(DEBUG_ENCODERS, "Starting left encoder");
            encLeft.start();

            d.dbg(DEBUG_ENCODERS, "Starting right encoder");
            encRight.start();
        }
    }

    public void doJoystickDataOps() {
        d.dbg(DEBUG_JOYSTICK, "Joystick Position");
        d.dbg(DEBUG_JOYSTICK, "X: " + joy.getX());
        d.dbg(DEBUG_JOYSTICK, "Y: " + joy.getY());
    }
    
    public void doEncoderDataOps() {
        if(ENCODER_SYS_ON) {
            /*	ENCODER stuff for wheel rotation detection */
            d.dbg(DEBUG_ENCODERS, "Right Encoder:");
            d.dbg(DEBUG_ENCODERS, "distance = " + encRight.getDistance());
            d.dbg(DEBUG_ENCODERS, "speed = " + encRight.getRate());
            d.dbg(DEBUG_ENCODERS, "Left Encoder:");
            d.dbg(DEBUG_ENCODERS, "distance = " + encLeft.getDistance());
            d.dbg(DEBUG_ENCODERS, "speed = " + encLeft.getRate());
        }
    }
    
    public void doAlternateFiringOps() {
        if(PNEUMATICS_SYS_ON) {
            if (joy.getRawButton(KICK_ALT)) {
                d.dbg(DEBUG_PNEUMATICS, "Kicking by alternate kick button");
                kickSolFire.set(Relay.Value.kForward);
                kickSolRetract.set(Relay.Value.kOff);
            }

            if (joy.getRawButton(RETRACT_ALT)) {
                d.dbg(DEBUG_PNEUMATICS,
                                  "Retracting by alternate retract button");
                kickSolFire.set(Relay.Value.kOff);
                kickSolRetract.set(Relay.Value.kForward);
            }
        }
    }

    public void doAlternateBallControlOps() {
        if(BALL_CONTROL_SYS_ON) {
            if (joy.getRawButton(BALL_CONTROL_ON)) {
                d.dbg(DEBUG_BALL_CONTROL, "Enable");
                ballController.set(Relay.Value.kForward);
            }

            if (joy.getRawButton(BALL_CONTROL_OFF)) {
                d.dbg(DEBUG_BALL_CONTROL, "Disable");
                ballController.set(Relay.Value.kOff);
            }
        }
    }
    
    public void doRegularFiringOps() {
        if(PNEUMATICS_SYS_ON && joy.getRawButton(TRIGGER) && !fireThread.isAlive()) {
            fireThread = new FireThread(kickSolFire, kickSolRetract,
                                                            ballController);
            fireThread.start();
        }
    }

    public void doTrackingAlignOps() {
        if (TRACKING_SYS_ON) {
                alignWithTargets();
        }
    }

    public void doFullForwardOps() {
        if(ENCODER_SYS_ON && fullForwardEnabled) {
            if(Math.abs(joy.getY()) < 0.1) {
                cErrR = 0.0;
                cErrL = 0.0;
            }
            //if direction change, then zero the error compensation shit
            if(joy.getY() < 0 && joy.getY() / JOY_Y_MAX + cErrL > 0) {
                cErrR = 0.0;
                cErrL = 0.0;
            }
            if(joy.getY() > 0 && joy.getY() / JOY_Y_MAX + cErrL < 0) {
                cErrR = 0.0;
                cErrL = 0.0;
            }
            //if see difference in encoder rates, increment enc error shit
            if     (joy.getY() < 0 && encLeft.getRate() < encRight.getRate()){
                cErrR -= 0.01;
            }
            else if(joy.getY() < 0 && encLeft.getRate() > encRight.getRate()) {
                cErrL -= 0.01;
            }
            else if(joy.getY() > 0 && encLeft.getRate() < encRight.getRate()){
                cErrR += 0.01;
            }
            else if(joy.getY() > 0 && encLeft.getRate() > encRight.getRate()){
                cErrL += 0.01;
            }

            drive.tankDrive(joy.getY() / JOY_Y_MAX + cErrL,
                                        joy.getY() / JOY_Y_MAX + cErrR);
        } else {
            drive.arcadeDrive(joy);
        }

        if(ENCODER_SYS_ON && joy.getRawButton(FULL_FORWARD_ON)) {
            fullForwardEnabled = true;
            cErrR = 0.0;
            cErrL = 0.0;
        }
        if(ENCODER_SYS_ON && joy.getRawButton(FULL_FORWARD_OFF) && fullForwardEnabled) {
            fullForwardEnabled = false;
        }
    }
    
    public void doCameraOps() {
        if (CAMERA_SYS_ON) {
                d.dbg(DEBUG_CAMERA, "Updating camera view on dashboard");
                DriverStationLCD.getInstance().updateLCD();//update DS camera
        }
    }

    public void doHangerOps() {
        if(HANGER_SYS_ON) {
            /* check to see what button modes are active */
            if(!isHangMode && hangStick.getRawButton(MODE_HANG_ON)) {
                /* if hang mode is off and they want it on*/
                isHangMode   = true;
            }
            else if(isHangMode && hangStick.getRawButton(MODE_HANG_OFF)) {
                isHangMode = false;
            }

            if(isHangMode) {
                /* joystick on to elevator mode, Y axis controls speed
                 * of elevator.  both directions work
                 * make it so that up in elevator runs the wench back
                 * at a certain speed
                 */
                hangerDrive.tankDrive(hangStick, null);
            }
        }
    }

    public void doTrackingDataOps() {
        if (TRACKING_SYS_ON) {
            if (joy.getRawButton(8)) {
                d.dbg(DEBUG_GYRO, "Angle: " + gyro.getAngle());
                d.dbg(DEBUG_GYRO, "Analog voltage: "
                                      + gyroChannel.getVoltage());
                d.dbg(DEBUG_GYRO, "Analog accumulated: "
                                      + gyroChannel.getAccumulatorCount());
            }
            if (joy.getRawButton(9)) {
                d.dbg(DEBUG_GYRO, "Reset");
                gyro.reset();
                d.dbg(DEBUG_GYRO, "Analog accumulator reset");
                gyroChannel.resetAccumulator();
            }

            if (joy.getRawButton(3)) {
                d.dbg(DEBUG_TRACKING_PIDCONTROLLER,
                                              "Disabling turn controller");
                d.dbg(DEBUG_TRACKING_PIDCONTROLLER,
                                           "Setting lastTrigger to false");
                turnController.disable();
                lastTrigger = false;
            }
        }
    }

    private boolean around(double a, double b, double precision) {
        return (Math.abs(a - b) < precision);
    }

    private void alignWithTargets() {
        d.dbg(DEBUG_TRACKING, "Attempting target alignment");

        long startTime = Timer.getUsClock();
        if (!joy.getRawButton(ALIGN)) {
            // allow driving
            if (!lastTrigger) {
                d.dbg(DEBUG_TRACKING, "Not attempting to align");
                d.dbg(DEBUG_TRACKING, "Regular arcade drive instead");
                
                drive.arcadeDrive(joy);
            } else {
                d.dbg(DEBUG_TRACKING, "Attempting to align in some form");

                d.dbg(DEBUG_TRACKING_PIDCONTROLLER,
                                "TC Setpoint: " + turnController.getSetpoint());

                if (lastTrigger && around(gyro.getAngle(),
                                              turnController.getSetpoint(), 1.0)) {
                    d.dbg(DEBUG_TRACKING, "Finished. Turned to target");

                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Disabled turn controller");
                    turnController.disable();

                    d.dbg(DEBUG_TRACKING, "Set lastTrigger to false");
                    lastTrigger = false;

                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Reset setpoint");
                    turnController.setSetpoint(0.0);
                }

                // don't know if we really need this
                if (Math.abs(turnController.getSetpoint()) > 57.0) {
                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Setpoint is beyond 57.0 " +
                        "degrees. This may be trouble. Stopping tracking");

                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Disabled turn controller");
                    turnController.disable();

                    d.dbg(DEBUG_TRACKING, "Set lastTrigger to false");
                    lastTrigger = false;
                }
            }
        } else {
            d.dbg(DEBUG_TRACKING, "Track-align button pressed. Attempting");
            findtarget(startTime);
        }
    }

    private void findtarget(long startTime) {
        d.dbg(DEBUG_TRACKING, "Beginning findTarget");

        if (!lastTrigger) {
            d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Enable");
            turnController.enable();
            d.dbg(DEBUG_GYRO, "Reset");
            gyro.reset();
            d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Setting setpoint: " +
                                                                gyro.pidGet());
            turnController.setSetpoint(gyro.pidGet());
        }
        lastTrigger = true;
        try {
            if (cam.freshImage()) {// && turnController.onTarget()) {
                double gyroAngle = gyro.pidGet();
                ColorImage image = cam.getImage();
                Thread.yield();
                Target[] targets = Target.findCircularTargets(image);
                Thread.yield();
                image.free();
                if (targets.length == 0 || targets[0].m_score < kScoreThreshold) {
                    d.dbg(DEBUG_TRACKING, "No target found");
                    Target[] newTargets = new Target[targets.length + 1];
                    newTargets[0] = new Target();
                    newTargets[0].m_majorRadius = 0;
                    newTargets[0].m_minorRadius = 0;
                    newTargets[0].m_score = 0;
                    for (int i = 0; i < targets.length; i++) {
                        newTargets[i + 1] = targets[i];
                    }
                    trackerDashboard.updateVisionDashboard(0.0, gyro.getAngle(),
                                                          0.0, 0.0, newTargets);

                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Disable");
                    turnController.disable();

                    d.dbg(DEBUG_TRACKING, "Setting lastTrigger to false");
                    lastTrigger = false;

                    d.dbg(DEBUG_GYRO, "Reset");
                    gyro.reset();

                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Resetting setpoint");
                    turnController.setSetpoint(0.0);
                } else {
                    //System.out.println(targets[0]);
                    d.dbg(DEBUG_TRACKING, "Target Angle: " +
                                              targets[0].getHorizontalAngle());

                    d.dbg(DEBUG_TRACKING_PIDCONTROLLER, "Setting setpoint to " +
                                                              "target angle: " +
                                (gyroAngle + targets[0].getHorizontalAngle()));
                    turnController.setSetpoint(gyroAngle +
                                               targets[0].getHorizontalAngle());

                    d.dbg(DEBUG_GYRO, "Gyro Angle: " + gyroAngle);
                    d.dbg(DEBUG_TRACKING, "Turn To Angle: " +
                                              targets[0].getHorizontalAngle());
                    d.dbg(DEBUG_TRACKING, "Difference: " +
                        Math.abs(gyroAngle - targets[0].getHorizontalAngle()));

                    d.dbg(DEBUG_TRACKING, "Updating trackerDashboard");
                    trackerDashboard.updateVisionDashboard(0.0, gyro.getAngle(),
                           0.0, targets[0].m_xPos / targets[0].m_xMax, targets);
                }
            }
        } catch (NIVisionException ex) {
            ex.printStackTrace();
        } catch (AxisCameraException ex) {
            ex.printStackTrace();
        }
        d.dbg(DEBUG_TRACKING, "Time: " +
                                (Timer.getUsClock() - startTime) / 1000000.0);
        d.dbg(DEBUG_GYRO, "Gyro Angle: " + gyro.getAngle());
    }
}
